from VehicleModule import TwoDofKinematicModel
from Plot import MapPlot
import matplotlib.pyplot as plt
import numpy as np
import DebugPlot
if __name__ == "__main__":
    '''init map'''
    obstacles = [(2, 2), (2, 3), (3, 3), (4, 4), (5, 5), (7, 7), (7, 8)]
    PlotVehicleMap = MapPlot(grid_size=(50, 50), obstacles=obstacles) 
    PlotVehicleMap.set_ego_car_position(start_pos=(1, 1), goal_pos=(8, 8))
    '''init car'''
    vehicle = TwoDofKinematicModel(x=0, y=0, theta=0, v=5.0, L=2.5, dt=0.1)
    acceleration=0
    delta = np.radians(10)
    for i in range(50):
        new_state = vehicle.update(acceleration, delta)
        new_pos = (new_state[0], new_state[1])
        PlotVehicleMap.update_car_position(new_pos)
        plt.pause(0.1)  # 暂停1秒看效果
    hisPath = vehicle.get_history()
    hisPath_xy = hisPath[:,0:2]
    speeds=hisPath[:,3]
    # print(hisPath_xy)
    PlotVehicleMap.update_path(hisPath_xy)
    DebugPlot.plot_debug(speeds,"speed")
    plt.show(block=True)


